#include "lsm6dsoxtr.h"


LSM6DSOXTR_Data_t lsm_data;


// 向传感器写入单个寄存器
static HAL_StatusTypeDef LSM_WriteReg(uint8_t reg, uint8_t data) {
  return HAL_I2C_Mem_Write(&hi2c1, LSM6DSOXTR_ADDR, reg, 
                          I2C_MEMADD_SIZE_8BIT, &data, 1, 100);
}

// 从传感器读取多个寄存器
static HAL_StatusTypeDef LSM_ReadRegs(uint8_t reg, uint8_t *buf, uint16_t len) {
  return HAL_I2C_Mem_Read(&hi2c1, LSM6DSOXTR_ADDR, reg, 
                         I2C_MEMADD_SIZE_8BIT, buf, len, 100);
}

// 初始化传感器（仅配置必要参数）
HAL_StatusTypeDef LSM6DSOXTR_Init(void) {
  uint8_t who_am_i;
  
  // 1. 检查设备连接
  if (LSM_ReadRegs(0x0F, &who_am_i, 1) != HAL_OK)
    return HAL_ERROR;
  if (who_am_i != 0x6C)  // 设备 ID 验证
    return HAL_ERROR;
  
  // 2. 配置加速度计：±4g 量程，104Hz 输出率
  if (LSM_WriteReg(0x10, 0x40 | 0x10) != HAL_OK)  // CTRL1_XL: ODR=104Hz, FS=±4g
    return HAL_ERROR;
  
  // 3. 配置陀螺仪：±500dps 量程，104Hz 输出率
  if (LSM_WriteReg(0x11, 0x40 | 0x01) != HAL_OK)  // CTRL2_G: ODR=104Hz, FS=±500dps
    return HAL_ERROR;
  
  // 4. 启用数据块更新（避免读取半字节）
  return LSM_WriteReg(0x12, 0x04);  // CTRL3_C: BDU=1
}

// 读取传感器数据
HAL_StatusTypeDef LSM6DSOXTR_ReadData(void) {
  uint8_t raw_data[12];
  
  // 读取加速度计数据（6字节：0x28-0x2D）
  if (LSM_ReadRegs(0x28, raw_data, 6) != HAL_OK)
    return HAL_ERROR;
  
  // 转换加速度计数据（±4g 量程：0.122mg/LSB → 0.000122g/LSB）
  int16_t ax = (raw_data[1] << 8) | raw_data[0];
  int16_t ay = (raw_data[3] << 8) | raw_data[2];
  int16_t az = (raw_data[5] << 8) | raw_data[4];
  lsm_data.accel_x = ax * 0.000122f;
  lsm_data.accel_y = ay * 0.000122f;
  lsm_data.accel_z = az * 0.000122f;
  
  // 读取陀螺仪数据（6字节：0x22-0x27）
  if (LSM_ReadRegs(0x22, raw_data, 6) != HAL_OK)
    return HAL_ERROR;
  
  // 转换陀螺仪数据（±500dps 量程：17.5mdps/LSB → 0.0175dps/LSB）
  int16_t gx = (raw_data[1] << 8) | raw_data[0];
  int16_t gy = (raw_data[3] << 8) | raw_data[2];
  int16_t gz = (raw_data[5] << 8) | raw_data[4];
  lsm_data.gyro_x = gx * 0.0175f;
  lsm_data.gyro_y = gy * 0.0175f;
  lsm_data.gyro_z = gz * 0.0175f;
  
  return HAL_OK;
}




